#pragma once
#include <memory>
#include <map>
namespace axisgroup {

class Robot;

enum class PlanMovingStatus
{
    MOVING_RUNNING,
    MOVING_STOPPING,
    MOVING_ERRSTOPPING,
    MOVING_STOPED,
};

struct PlanStatus
{
    int errcode = 0;
    PlanMovingStatus status = PlanMovingStatus::MOVING_STOPED;
};

class PFsmState
{
public:
    enum class PlanMode
    {
        PLAN_IDLE,
        PLAN_FREE,
        PLAN_SERVO,
        PLAN_DRAG,
        PLAN_COORD,
    } plan_type_ = PlanMode::PLAN_IDLE;

    static std::map<PlanMode, std::string> PLAN_MODE_NAME;

public:
    PFsmState(Robot* grp);
    virtual ~PFsmState() = default;
    virtual int init();
    int enter_mode();
    void exit_mode();
    PlanStatus update();
    virtual void pause();
    virtual void resume();
    virtual bool is_paused() const { return is_paused_; }
    void CAT2_stop();   // 错误停止
    void abort_plan();  // 正常停止
    bool is_inpos() const { return inpos_; }

protected:
    PlanMovingStatus check_inpos();
    virtual bool is_fdb_inpos();
    virtual int enter() = 0;
    virtual void exit() = 0;
    virtual void abort() = 0;
    virtual bool is_plan_over() = 0;
    virtual int update_plan() = 0;
    virtual bool is_stopping() = 0;

protected:
    Robot* grp_;
    bool is_paused_ = false;

private:
    PlanStatus status_;
    bool inpos_ = true;
};

class PFsmIdle : public PFsmState
{
public:
    PFsmIdle(Robot* grp);
    virtual ~PFsmIdle() = default;
    virtual int enter() override;
    virtual void exit() override { return; }
    virtual void abort() override { return; }
    virtual bool is_plan_over() override { return true; }
    virtual bool is_fdb_inpos() override { return true; }
    virtual bool is_stopping() override { return false; }
    virtual int update_plan() override;
};

std::shared_ptr<PFsmState> creat_pfsm(Robot* grp, PFsmState::PlanMode mode);

}  // namespace axisgroup